package org.usfirst.frc1305;

//imports from WPILib
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Encoder.PIDSourceParameter;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Relay.Value;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SendablePIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
//other imports in this package
import org.usfirst.frc1305.Ports;
import org.usfirst.frc1305.Constants;

/**
 * Code for the 2012 robot, Magnesium
 *  
 * @author Paul Belanger <paulirino@gmail.com>
 * @version 1.0.3
 * @since 2012-04-03
 */

public class Magnesium extends SimpleRobot
{
	//Set up all of the Victors
	
	//Drive Victors
	Victor leftDrive1  		= new Victor(Ports.PWM_LEFT_MOTOR1);
	Victor leftDrive2  		= new Victor(Ports.PWM_LEFT_MOTOR2);
	Victor rightDrive1 		= new Victor(Ports.PWM_RIGHT_MOTOR1);
	Victor rightDrive2 		= new Victor(Ports.PWM_RIGHT_MOTOR2);
	
	//Other Motor Victors
	Victor mIntakeArm  		= new Victor(Ports.PWM_INTAKE_ARM);
	Victor mHoodTrim   		= new Victor(Ports.PWM_HOOD_TRIM);
	Victor mShooter1  		= new Victor(Ports.PWM_SHOOTER1);
	Victor mShooter2 		= new Victor(Ports.PWM_SHOOTER2);
	Victor mHoodRotation 	= new Victor(Ports.PWM_HOOD_ROTATION);
	
	//Set up the Relays
	Relay rRollerIntake = new Relay(Ports.RELAY_INTAKE_ROLLER);
	Relay rRollerTower 	= new Relay(Ports.RELAY_TOWER_ROLLER);
	Relay rCameraLights = new Relay(Ports.RELAY_CAMERALIGHTS);
	
	//Set up the Camera Servo
	Servo mCameraServo		= new Servo(Ports.PWM_CAMERA_SERVO);
	
	//Set up Encoders. (Left and Right wheel Encoders currently unused)
	Encoder encLeftWheel 	= new Encoder(Ports.DIO_ENC_LEFTWHEEL1, Ports.DIO_ENC_LEFTWHEEL2);
	Encoder encRightWheel	= new Encoder(Ports.DIO_ENC_RIGHTWHEEL1, Ports.DIO_ENC_RIGHTWHEEL2);
	Encoder encTowerRotation= new Encoder(Ports.DIO_ENC_TOWER1, Ports.DIO_ENC_TOWER2);
	Encoder encRPMSensor	= new Encoder(Ports.DIO_ENC_RPMSENSOR, Ports.DIO_ENC_RPMSENSOR2); //Ports one and two are defined as the same port 
																							  //Direction is not required therefore Port 2 is unused
	
	//Set up Analog Sensors
	AnalogChannel potHoodTrim 		= new AnalogChannel(Ports.AIO_HOODTRIM);
	AnalogChannel potIntakeWheel	= new AnalogChannel(Ports.AIO_INTAKEWHEEL);
//	Gyro gyroRotation 				= new Gyro(Ports.AIO_GYRO); //The Gyro is currently unused 
	
	//Set up Solenoids
	Solenoid solShifter		= new Solenoid(Ports.SOL_GEARSHIFT);
	Solenoid solTowerPlate	= new Solenoid(Ports.SOL_TOWERPLATE);
	Solenoid solIntake		= new Solenoid(Ports.SOL_INTAKE);
	Solenoid solLightSensor = new Solenoid(Ports.SOL_LIGHT_SENSOR);
	
	//Set up PID controllers
	PIDController pidHoodTrim 	  =	new PIDController(0.00925, 0.0009, 0.0000006, potHoodTrim, mHoodTrim);
	PIDController pidHoodRotation = new PIDController(0.014, 0.00049, 0.000081, encTowerRotation,mHoodRotation);	
	PIDController pidShooterWheel = new PIDController(0.0, 0.0, 0.0, encRPMSensor, mShooter1);
	
	//Robot Drive System (Given references to the four motors that it controls)
	RobotDrive chassis 		= new RobotDrive(leftDrive1, leftDrive2, rightDrive1, rightDrive2);
	
	//Set up Joysticks
	Joystick baseControl 	= new Joystick(1);
	Joystick gunnerControl	= new Joystick(2);
	
	//Set up Compressor (Automatically controlled through pressure switch) 
	Compressor compressor 	= new Compressor(Ports.DIO_PRESSURESENSOR, Ports.RELAY_COMPRESSOR);
	
	//Variables used for control
	private boolean currentGear; //Holds drive gear state (True = High Gear)(False = Low Gear)
	private boolean intakeTrimOverride; //Overrides intake trim function
	private double  camPosition; //Position of the Camera Servo
	private boolean oneTime = true; //Flag that handles run-once autonomous commands
	private double output;
	
	private double Error;
	private double Setpoint = 0;
	private double OutputSpeed = 0.25;
	private int counter = 0;
	private double rate;
	private double StartTime = 0;
	private boolean ticker = true;
	
	
	
	public Magnesium()//Set start-up variables
	{ 
		
		//Turn on compressor function
		compressor.start();
		
		//Set the chassis expiration(In case of endless loop, drive motors will shut down in 0.1 seconds)
		chassis.setExpiration(0.1);
		
		//Defaults for various settings
		currentGear = false;
		intakeTrimOverride = false;
		camPosition = 0.80;
		mCameraServo.set(camPosition);
		
		//Set tolerance percentage for PID shooter wheel
		pidShooterWheel.setTolerance(2.0);
		
		//RPM sensor constants setup and start
		encRPMSensor.setDistancePerPulse(1.0/32.0);
		encRPMSensor.setReverseDirection(true);
		encRPMSensor.setPIDSourceParameter(PIDSourceParameter.kRate);
		encRPMSensor.start();
		
		//Set the tower encoder to zero
		encTowerRotation.reset();
			
		//Set up done
		System.out.println("INFO: init finished");
	}
	
	/**
	 * the autonomous function
	 * 
	 * This function is called once at the start of the autonomous period.
	 * The while loop makes sure it only runs as long as the robot is in autonomous mode. 
	 */
	
	public void autonomous()
	{
		//Turn off motor safety
		chassis.setSafetyEnabled(false);
        
        while(isAutonomous())
        {
        	if(oneTime == true)  //Occur only once
        	{ 
        		//Lower the intake roller and turn on
        		SetIntakeBeltState(true);
        		SetIntakeRollerTrim(false, true);
        		Timer.delay(0.35);
        		SetIntakeRollerTrim(false, false);
        	}
        	
        	oneTime = false;
        	
        	//Turn on shooter
        	SetShooterWheelSpeed(false, true, false, false); 
        	Timer.delay(1.0);
        	
        	//Turn on tower belt
        	SetTowerBeltState(true, false);
        	Timer.delay(0.5);
        	
        	//Close Platten
        	SetTowerPistonState(true);
        }
    }
	
	/**
	 * the operator control function
	 * 
	 * This function is called once at the beginning of the operatorControl part of the game. 
	 * The loops ensure that it only works when the robot is enabled and that it doesn't run when it isn't supposed to. 
	 */
	
    public void operatorControl() 
    {
    	System.out.println("INFO: entering OperatorControl");
    	//Turn on motor safety
    	chassis.setSafetyEnabled(true); 
    	
    	while(isOperatorControl())
    	{
    		
    		while(isEnabled())
    		{    			
    		    //--NON-DRIVEN FUNCTIONS--			
    			//Update the Smart Dashboard
    			updateDashboard();

    			//--MAIN DRIVER FUNCTIONS--
    			//drive using the two analog sticks
    			chassis.arcadeDrive(JoystickSmoothing(baseControl.getRawAxis(2)), JoystickSmoothing(baseControl.getRawAxis(3)));
    			
    			//set the gear using right shoulders
    			SetChassisGear(baseControl.getRawButton(6), baseControl.getRawButton(8));
    			
    			//set input trim using lleft triggers
    			SetIntakeRollerTrim(baseControl.getRawButton(5), baseControl.getRawButton(7));
    			
    			//emergency intake piston drop using the button 4(y)
    			SetIntakePistonState(gunnerControl.getRawButton(4));

    			//--SECONDARY DRIVER FUNCTIONS--    			
    			//intake belt state toggle
    			SetIntakeBeltState(gunnerControl.getRawButton(6));
    			
    			//hood trim using right analog up/down
//    			SetHoodTrim(-1.0*gunnerControl.getRawAxis(4));	//Unused because hood trim was removed
    			
    			//camera elevation
    			SetCameraElevation(gunnerControl.getRawAxis(2));
    			
    			//shoot wheel velocityy
//    			SetShootWheelVelocity(shooterstick.getRawButton(3), shooterstick.getRawButton(1),shooterstick.getRawButton(2), shooterstick.getRawButton(4));		//unused because PID used in place
    			
    			//Set tower rotation
    			SetTowerRotation(gunnerControl.getRawAxis(1), gunnerControl.getRawButton(0));
    			
    			//toggle the backplate
    			SetTowerPistonState(gunnerControl.getRawButton(7));
    			
    			//elevator on button 8
    			SetTowerBeltState(gunnerControl.getRawButton(8), gunnerControl.getRawButton(5));
    			
    			//and finally a really ghetto pid
  //  			SetShooterWheelSpeed(gunnerControl.getRawButton(2), gunnerControl.getRawButton(3), gunnerControl.getRawButton(1),gunnerControl.getRawButton(4));
    			
    			ShooterPID(gunnerControl.getRawButton(1),gunnerControl.getRawButton(2), gunnerControl.getRawButton(3), gunnerControl.getRawButton(4));
    		
    			//DataCollection(gunnerControl.getRawButton(2));
    		}
    	}
    }
    
	/** 
     * update the Smart Dashboard with values. Should be run once per loop. 
     */
    
    void updateDashboard()
    {
    	//Shooter Motor info
    	if(encRPMSensor.getRate() < 100)
    		SmartDashboard.putDouble("Shooter Wheel Current:", encRPMSensor.getRate());
       	
    	SmartDashboard.putInt("Shooter Voltage:", (int) (-100.0*mShooter1.get()));
       	
    	if (Setpoint - encRPMSensor.getRate() > -5 && Setpoint - encRPMSensor.getRate() < 5)
    		SmartDashboard.putString("Lock On:", "Fire!");
    	else if(gunnerControl.getRawButton(3) && encRPMSensor.getRate() < 70)
    		SmartDashboard.putString("Lock On:", "Power of the horse!");
    	else if(gunnerControl.getRawButton(3) && encRPMSensor.getRate() > 70)
    		SmartDashboard.putString("Lock On:", "FULL FORCE");
    	else
    		SmartDashboard.putString("Lock On:", "Wait");
    		
       	
    }
    
    /**
     * Sets the shoot wheel speed to the desired speed. The speed is controlled by a PIDController
     * @param value the speed is revolutions per second to set the wheels to.
     */
    void SetShootWheelSpeed(double value)
    {
    	 
    	if(gunnerControl.getRawButton(2))
    		pidShooterWheel.setPID(1, 0, 0);
    	else if(gunnerControl.getRawButton(1))
    		pidShooterWheel.setPID(300, 5, 20);
    	else if(gunnerControl.getRawButton(4))
    		pidShooterWheel.setPID(300,0,60);
    	
    	pidShooterWheel.setSetpoint(value);
    	
    	pidShooterWheel.setOutputRange(0, 12);
    	SmartDashboard.putDouble("Error:", pidShooterWheel.getError());
    	SmartDashboard.putDouble("input:", encRPMSensor.pidGet());
    	
    	System.out.println(pidShooterWheel.get()/10);
    	
    	output = -1 * (pidShooterWheel.get()/10);
    
    	if(output > 1)
    		output = 1;
    	else if (output < -1)
    		output = -1;
    	
    	mShooter1.set(output);
    }
    
    /**
     * Simple joystick smoothing function that squares the joystick values while preserving the sign.
     * @author Wesley Groom
     * @param stick the joystickvalue to attempt to smooth
     */
    
    double JoystickSmoothing(double stick)
    {
      if( stick < 0 )
        return -1.0 * stick * stick;
      else
        return stick * stick;
    } 

    /**
     * Set the pistons controlling the chassis drive speed. 
     * @param upstate shift and hold the chassis in high gear
     * @param downstate shift and hold the chassis in low gear
     */
    
    void SetChassisGear(boolean upstate, boolean downstate)
    {
    	if(upstate) 		
    		currentGear = true;
    	else if(downstate) 	
    		currentGear = false;
    	
    	solShifter.set(currentGear);
    }
    
    /**
     * Sets the trim of the intake roller on the robot. This method is disabled when SetIntakePistonState is set to True. 
     * @param upbutton Bring the intake roller up. Takes precedence over downbutton
     * @param downbutton Bring the intake roller down. 
     */
    
    void SetIntakeRollerTrim(boolean upbutton, boolean downbutton)
    {
    	if(downbutton) 
    		mIntakeArm.set(0.5);
    	else if(upbutton)
    		mIntakeArm.set(-1.0);
    	else 
    		mIntakeArm.set(0.0);    	
    }
    
    /**
     * control the intake roller piston. This function overrides SetIntakeRollerTrim. 
     * @param state whether or not the intake piston should be extended
     */
    
    void SetIntakePistonState(boolean state)
    {
    	if(state)
    	{
    		intakeTrimOverride = true;
    		mIntakeArm.set(0.9);
    		solIntake.set(true);
    	}
    	else if(intakeTrimOverride)
    	{
    		mIntakeArm.set(0.0);
    		solIntake.set(false);
    		intakeTrimOverride = false;
    	}
    }

    /**
     * operate Belts on the intake roller
     * @param state whether the belts are moving or not
     */
    
    void SetIntakeBeltState(boolean state)
    {
    	if(state) 	
    		rRollerIntake.set(Value.kForward);
    	else 
    		rRollerIntake.set(Value.kOff);
    }   
    
    /**
     * sets the trim of the shooter hood. The trim is capped using a potientometer. 
     * @param value the joystick value that controls the hood trim. 
     */
    
    void SetHoodTrim(double value)
    {
    	//proportional constants to slow down the motor
    	if(value > 0.0) value *= 0.4;
    	if(value < 0.0) value *= 0.3;
    	
    	//we need to keep the hood trim within allowable limits using a potientometer
    	if(potHoodTrim.getValue() > Constants.HOODTRIM_MIN_VALUE + 20 && potHoodTrim.getValue() < Constants.HOODROTATION_MAX_VALUE - 20)
    	mHoodTrim.set(value);
    	else if(potHoodTrim.getValue() > Constants.HOODTRIM_MIN_VALUE + 20 && value < 0)
    		mHoodTrim.set(value);
    	else if(potHoodTrim.getValue() < Constants.HOODTRIM_MAX_VALUE - 20 && value > 0)
    		mHoodTrim.set(value);
    	else
    		mHoodTrim.set(0);
    }
    
    /**
     * control the trim of the camera servo
     * @param stick the joystick value that controls the camera trim. 
     */
    
    void SetCameraElevation(double stick)
    {
    	if(stick > 0.05 || stick < -0.05)
    	{
    		double dChangeFactor = -1 * stick / 100;
    		camPosition += dChangeFactor;
    		if(camPosition > 1.0) 
    			camPosition = 1.0;
    		if(camPosition < 0.5) 
    			camPosition = 0.5;
    		
    		mCameraServo.set(camPosition);
    	}
    }
    
    /**
     * Set the speed of the shooter wheel based on raw voltage percentages. Actual speed is affected by current battery voltage. 
     * @param but1 sets the shooter wheel to full speed. 
     * @param but2 sets the shooter wheel to 83% speed
     * @param but3	sets the shooter wheel to 27% speed. 
     * @param but4 sets the shooter wheel to 33.5% speed. 
     */
    void SetShootWheelVelocity(boolean but1, boolean but2, boolean but3, boolean but4)
    {
    	if(but1)
    	{
    		mShooter1.set(-1.0);
    		mShooter2.set(-1.0);
    	}
    	else if(but2)
    	{
    		mShooter1.set(-0.83);
    		mShooter2.set(-0.83);
    	}
    	else if(but3)
    	{
    		mShooter1.set(-0.27);
    		mShooter2.set(-0.27);
    	}
    	else if(but4)
    	{
    		mShooter1.set(-0.335);
    		mShooter2.set(-0.335);
    	}
    	else
    	{
    		mShooter1.set(0);
    		mShooter2.set(0);
    	}
    }
    
    /**
     * set the rotation of the shooter mechanism. 
     * @param magnitude the value from the joystick that controls the rotation
     * @param zerooverride overrides magnitude and attempts to return the rotation to the 'zero' position
     */
    
    void SetTowerRotation(double magnitude, boolean zerooverride)
    {
    	//currently this thing just sets the motor to the value given to it by the joystick
    	if(!zerooverride)
    		mHoodRotation.set(magnitude);
    	else if(zerooverride){
    		//TODO: turret centering here
    	}
    }
    
    /**
     * controls the back panel on the tower
     * @param state the state of the piston. false is out, true is in. 
     */
    
    void SetTowerPistonState(boolean state)
    {
    	solTowerPlate.set(!state);
    }
    
    /**
     * Control the polycord belts running up the tower
     * @param upstate set the belts to travel upwards. Takes precidence over downstate
     * @param downstate set the belts to travel downwards. 
     */
    
    void SetTowerBeltState(boolean upstate, boolean downstate)
    {
    	if(upstate)	
    		rRollerTower.set(Value.kReverse);
    	else if(downstate)	
    		rRollerTower.set(Value.kForward);
    	else	
    		rRollerTower.set(Value.kOff);
    }   
 
    /**
     * A helper function to translate button presses into shooter wheel speeds. 
     * @param button1 set the shooter wheel to 73rps
     * @param button2 set the shooter wheel to 64rps
     * @param button3 set the shooter wheel to 20rps
     * @param button4 set the shooter wheel to 26rps
     */
    
    void SetShooterWheelSpeed(boolean button1, boolean button2, boolean button3, boolean button4)
    {
    	if(button1)
    	{
    		pidShooterWheel.enable();
    		SetShootWheelSpeed(25.0);
    		SmartDashboard.putString("State:", "25");
    	}
    	else if(button2)
    	{
    		mShooter1.set(-1);
    		SmartDashboard.putString("State:", "Power of the horse");
    	}
    	else if(button3)
    	{
    		pidShooterWheel.enable();
    		SetShootWheelSpeed(40.0); 
    		SmartDashboard.putString("State:", "40");
    	}
    	else if(button4)
    	{
    		pidShooterWheel.enable();
    		SetShootWheelSpeed(65.0);  
    		SmartDashboard.putString("State:", "65");
    	}
    	else
    	{
    		SmartDashboard.putString("State:", "Off");
    		mShooter1.set(0.0);
    		pidShooterWheel.reset();
    	}
    }
    void ShooterPID (boolean button1, boolean button2, boolean button3, boolean button4)
    {    	
    	SmartDashboard.putDouble("Speed", encRPMSensor.getRate());
    	
    	if(button1)
    	{    		
    		Setpoint = 25;
    		
    		if(ticker)
    		{
    			mShooter1.set(-1);
    			OutputSpeed = .19;
    			ticker = false;
    		}
    		
    		if(StartTime == 0)
    			StartTime = System.currentTimeMillis();
   		
    		if(System.currentTimeMillis() - StartTime > 260)
    		{    			
    			rate = encRPMSensor.getRate();
    			
    			if(rate < 100)
    			{    				
    				++counter;
    				
    				if(counter == 8)
    				{        			
        				Error = Setpoint - rate;
        		
        				if(Error > 1)
        					OutputSpeed = OutputSpeed + .01;
        				else if(Error < -1)
        					OutputSpeed = OutputSpeed - .01;
        				
        				counter = 0;
        			}
    				mShooter1.set(-1 * OutputSpeed);
        		}    		
    		}
    	}
    	else if(button2)
    	{    		
    		Setpoint = 80;
    		
    		if(ticker)
    		{
    			mShooter1.set(-1);
    			OutputSpeed = .75;
    			ticker = false;
    		}
    		
    		if(StartTime == 0)
    			StartTime = System.currentTimeMillis();
   		
    		if(System.currentTimeMillis() - StartTime > 1200)
    		{    			
    			rate = encRPMSensor.getRate();
    			
    			if(rate < 100)
    			{    				
    				++counter;
    				
    				if(counter == 8)
    				{        			
        				Error = Setpoint - rate;
        		
        				if(Error > 1)
        					OutputSpeed = OutputSpeed + .01;
        				else if(Error < -1)
        					OutputSpeed = OutputSpeed - .01;
        				
        				counter = 0;
        			}
    				mShooter1.set(-1 * OutputSpeed);
        		}    		
    		}
    	}
    	else if(button3)
    	{    		
    		OutputSpeed = 1;
    		mShooter1.set(-1 * OutputSpeed);
    	}
    	else if(button4)
    	{    		
    		Setpoint = 40;
    		
    		if(ticker)
    		{
    			mShooter1.set(-1);
    			OutputSpeed = .28;
    			ticker = false;
    		}
    		
    		if(StartTime == 0)
    			StartTime = System.currentTimeMillis();
   		
    		if(System.currentTimeMillis() - StartTime > 500)
    		{    			
    			rate = encRPMSensor.getRate();
    			
    			if(rate < 100)
    			{    				
    				++counter;
    				
    				if(counter == 8)
    				{        			
        				Error = Setpoint - rate;
        		
        				if(Error > 1)
        					OutputSpeed = OutputSpeed + .01;
        				else if(Error < -1)
        					OutputSpeed = OutputSpeed - .01;
        				
        				counter = 0;
        			}
    				mShooter1.set(-1 * OutputSpeed);
        		}    		
    		}
    	}
    	else{
    		ticker = true;
    		OutputSpeed = 0;
    		mShooter1.set(-1 * OutputSpeed);
    		StartTime = 0;
    	}
    	
    	
    }
    
    void DataCollection(boolean button)
    {
    	if(button)
    		mShooter1.set(-1);
    	else
    		mShooter1.set(0.0);
    }
}
